#include<stdio.h>
#include<unistd.h>
#include<signal.h>
#include<stdlib.h>

#include"pca9685/pca9685.h"
#include<global_header.h>
#include"wflyctrl.h"

/* checked by oscillograph.
 * @og_ph_of,
 *
 * as [ 438/4095 == 2.3ms/21.5ms == 0.0709 ].
 * 2.3ms is the wfly-output-PWM og_ph_of,
 * checked by oscillograph.
 *
 * @lowest,
 * as [ 194/4095 == 0.0473 ]
 *
 * @highest,
 * as [ 387/4095 == 0.0945 ]
 *
 * Only if `i' >= 208
 * then pwm_sgrt_setpwmout_off(WFLY_ROCKER_ACCEL,
 *      WFLY_ROCKER_ACCEL * og_ph_of + i);
 * then plane will not shutdown the accelerator automatically.
 * Normally, the plane will shutdown the accelerator,
 * after 3 second if [ `i' < 208 ].
 *
 * 313 is the special accelerator value
 */
#define og_ph_of (438)  // original_phase_offset
#define median (290)    // median rocker in wfly
#define lowest (194)    // lowest rocker in wfly
#define highest (387)   // highest rocker in wfly
/* [ 387 - 194 == 193 ], 193 is closed to 192. */
#define WFLY_LOW(x)  pwm_sgrt_setPWMout_off(x, x * og_ph_of + lowest)
#define WFLY_MID(x)  pwm_sgrt_setPWMout_off(x, x * og_ph_of + median)
#define WFLY_HIGH(x) pwm_sgrt_setPWMout_off(x, x * og_ph_of + highest)

static int accelerator_value = 0;

enum WFLY_ROCKER {
    WFLY_ROCKER_DIRLR,  // direction left and right
    WFLY_ROCKER_ACCEL,  // accelerator
    WFLY_ROCKER_DIRFB,  // direction forward and backward
    WFLY_ROCKER_SPIN,    // spin
};

static int succeed_init_flag = 0;
#define DEV_NAME "wfly: "

/* to control the ping_pong_box */
int ping_pong_box_open(void) {
    if(!succeed_init_flag) {
        fprintf(stderr, DEV_NAME
                "wfly does not initialized, %s.\n",
                __func__);
        return -1;
    }
    pwm_sgrt_setPWMout(4, 0, 160);
}

int ping_pong_box_close(void) {
    if(!succeed_init_flag) {
        fprintf(stderr, DEV_NAME
                "wfly does not initialized, %s.\n",
                __func__);
        return -1;
    }
    pwm_sgrt_setPWMout(4, 0, 560);
}

unsigned char wfly_stop(void) {
    if(!succeed_init_flag) {
        fprintf(stderr, DEV_NAME
                "wfly does not initialized, %s.\n",
                __func__);
        return -1;
    }
    WFLY_LOW(WFLY_ROCKER_ACCEL);
    abs_usleep(3*1000*1000);

    /* re-back the stable state */
    WFLY_MID(WFLY_ROCKER_DIRLR);
    WFLY_MID(WFLY_ROCKER_DIRFB);
    WFLY_MID(WFLY_ROCKER_SPIN);

    return 0;
}

void atexit_wfly_stop(void) {
    wfly_stop();
}

unsigned char wfly_init(void) {
    if(succeed_init_flag) {
        fprintf(stderr, DEV_NAME
                "wfly has already initialized, %s.\n",
                __func__);
        return 0;
    }

    unsigned char ret;
    pwm_sgrt_init();
    pwm_sgrt_reset();

    /* the output signals of wfly,
     * f=47.2Hz, T=21.5ms, in  oscillometer.
     *
     * But as the inaccuracy of pca9685,
     * we have to declare the f=44.22Hz to pca9685.
     * Only this it will output f=47.2Hz PWM effectively.
     */
    ret = pwm_sgrt_setfrq(44.22);

    /*
     * [ 290/4095 == 0.0708 ],
     * which is close to 0.079
     *
     * wfly will output 0.079 (the pwm-duty-factor) when
     * each rocker in wfly stays the middle-state.
     */
    unsigned short mid_duty_t = 290,
                   ont, offt;

    unsigned char i = 0;
    for(i = 0;i < 4;i ++) {
        ont = og_ph_of * i;
        offt = mid_duty_t + og_ph_of * i;
        ret |= pwm_sgrt_setPWMout(i, ont, offt);
    }
    succeed_init_flag = 1;
    ping_pong_box_close();
    atexit(atexit_wfly_stop);
    return ret;
}

unsigned char wfly_lauch(void) {
    if(!succeed_init_flag) {
        fprintf(stderr, DEV_NAME
                "wfly does not initialized, %s.\n",
                __func__);
        return -1;
    }

    WFLY_HIGH(WFLY_ROCKER_DIRLR);
    WFLY_LOW(WFLY_ROCKER_ACCEL);
    WFLY_LOW(WFLY_ROCKER_DIRFB);
    WFLY_HIGH(WFLY_ROCKER_SPIN);
    abs_usleep(1*1000*1000); /* wait it for stable */

    WFLY_MID(WFLY_ROCKER_DIRLR);
    WFLY_MID(WFLY_ROCKER_DIRFB);
    WFLY_MID(WFLY_ROCKER_SPIN);
    return 0;
}

/* accelerator control interface */
int wfly_accel_up(void) {
    /* up the accelerator */

    static int func_mutex = 0; /* simple spin lock */

    if(func_mutex)
        return -1;
    func_mutex = 1; /* lock */

    /* check the input-param */

    pwm_sgrt_setPWMout_off(WFLY_ROCKER_ACCEL,
            WFLY_ROCKER_ACCEL * og_ph_of + 313);
    func_mutex = 0; /* unlock */
    return 0;
}

int wfly_accel_stay(void) {
    /* up the accelerator */

    static int func_mutex = 0; /* simple spin lock */

    if(func_mutex)
        return -1;
    func_mutex = 1; /* lock */

    /* check the input-param */

    pwm_sgrt_setPWMout_off(WFLY_ROCKER_ACCEL,
            WFLY_ROCKER_ACCEL * og_ph_of + 286);
    func_mutex = 0; /* unlock */
    return 0;
}

int wfly_accel_down(int val) {
    /* check the input-param */
    if(val > 280 || val < 260)
        return -1;
    static int func_mutex = 0; /* simple spin lock */
    int i;

    if(func_mutex)
        return -1;
    func_mutex = 1; /* lock */

    pwm_sgrt_setPWMout_off(WFLY_ROCKER_ACCEL,
            WFLY_ROCKER_ACCEL * og_ph_of + val);

    func_mutex = 0; /* unlock */
    return 0;

}

int wfly_fb_ctrl(int val) {
    if(val < -WFLY_FBLR_MAX_VAL || val > WFLY_FBLR_MAX_VAL)
        return -1;
    static int func_mutex = 0; /* simple lock */
    if(func_mutex)
        return -1;
    func_mutex = 1; /* lock */

    pwm_sgrt_setPWMout_off(WFLY_ROCKER_DIRFB,
            WFLY_ROCKER_DIRFB * og_ph_of + median + val);
    func_mutex = 0;
    return 0;
}

int wfly_lr_ctrl(int val) {
    if(val < -WFLY_FBLR_MAX_VAL || val > WFLY_FBLR_MAX_VAL)
        return -1;
    static int func_mutex = 0; /* simple lock */
    if(func_mutex)
        return -1;
    func_mutex = 1; /* lock */

    pwm_sgrt_setPWMout_off(WFLY_ROCKER_DIRLR,
            WFLY_ROCKER_DIRLR * og_ph_of + median + val);
    func_mutex = 0;
    return 0;

}
